#include "stepmotor_test.h"
#include "rs232_interface.h"
#include <math.h>
#include <stdlib.h>
#include <string.h>
#define TIM5_CCR2_Address    0x40010C38

uint16_t *SRC_Buffer;
__IO uint16_t prescaler = 1000;
__IO uint32_t pulse_count = 0; /*  脉冲计数，一个完整的脉冲会增加2 */



SpeedCalc_TypeDef Speed;
StepBuffer_TypeDef StepBuffer;
__IO int32_t  Step_Position = 0;           // 当前位置
__IO uint16_t Toggle_Pulse = 0;           // 脉冲频率控制
__IO uint8_t  MotionStatus = WAIT;        // 是否在运动？0：停止，1：运动, 2: 匀速, 3: 减速 


//
//条件：写buff有可用空间
uint8_t WriteBuff(float fBuffer)
{
    if(StepBuffer.BufferState[StepBuffer.WriteIndex] != FULL &&
        StepBuffer.BufferState[StepBuffer.WriteIndex] != WRITEOVER)
    {
        StepBuffer.Buffer[StepBuffer.WriteIndex][StepBuffer.BuffWriteIndex++] = fBuffer;
        StepBuffer.BufferState[StepBuffer.WriteIndex] = WRITING;
        
        if(StepBuffer.BuffWriteIndex == MAX_BUFF_SIZE)
        {
            StepBuffer.BufferState[StepBuffer.WriteIndex] = FULL;
        }
    }

}

void WriteOver()
{
    StepBuffer.BufferState[StepBuffer.WriteIndex] = WRITEOVER;
}
//条件：读buff数据已写满或数据传输完成
float* ReadBuff(uint16_t &sLen)
{
    float* pBuff = NULL;
    if(StepBuffer.BufferState[StepBuffer.WriteIndex] == FULL ||
        StepBuffer.BufferState[StepBuffer.WriteIndex] == WRITEOVER)
    {
        sLen = StepBuffer.BuffWriteIndex;
        StepBuffer.BufferState[StepBuffer.WriteIndex] = READING;
        pBuff = StepBuffer.Buffer[StepBuffer.WriteIndex];
        StepBuffer.ReadIndex = StepBuffer.WriteIndex;
    }
    return pBuff;
}

void ReadOver()
{
    StepBuffer.BufferState[StepBuffer.ReadIndex] = IDLE;
}
/**
  * 函数功能: 配置TIMx复用输出PWM时用到的I/O
  * 输入参数: 无
  * 返 回 值: 无
  * 说    明：无
  */
static void TB6600_GPIO_Config(void)
{
    GPIO_InitTypeDef GPIO_InitStructure;

    /* 使能定时器通道引脚GPIO时钟 */
    RCC_APB2PeriphClockCmd(TB6600_TIM_GPIO_CLK | TB6600_ENA_GPIO_CLK, ENABLE);

    /* 配置定时器通道1输出引脚模式：复用开漏输出模式 */
    GPIO_InitStructure.GPIO_Pin = TB6600_TIM_CH1_PIN;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF_OD;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(TB6600_TIM_CH1_PORT, &GPIO_InitStructure);

    GPIO_InitStructure.GPIO_Pin = TB6600_DIR_PIN;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_OD;
    GPIO_Init(TB6600_DIR_PORT, &GPIO_InitStructure);
    GPIO_SetBits(TB6600_DIR_PORT, TB6600_DIR_PIN);

    GPIO_InitStructure.GPIO_Pin = TB6600_ENA_PIN;
    GPIO_Init(TB6600_ENA_PORT, &GPIO_InitStructure);
    GPIO_SetBits(TB6600_ENA_PORT, TB6600_ENA_PIN);
}


/**
  * 函数功能: 配置TIM3输出的PWM信号的模式，如周期、极性、占空比
  * 输入参数: 无
  * 返 回 值: 无
  * 说    明：无
  */
  /*
   * TIMxCLK/CK_PSC --> TIMxCNT --> TIMx_ARR --> TIMxCNT 重新计数
   *                    TIMx_CCR(电平发生变化)
   * 信号周期=(TIMx_ARR +1 ) * 时钟周期
   * 占空比=TIMx_CCR/(TIMx_ARR +1)
   */
static void TB6600_TIMx_Configuration(void)
{
    TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
    TIM_OCInitTypeDef  TIM_OCInitStructure;
    /* 定义嵌套向量中断控制器初始化结构体变量 */
//    NVIC_InitTypeDef NVIC_InitStructure;

    /* 使能定时器始终：设置TIM3CLK 为 72MHZ */
    TB6600_TIM_APBxClock_FUN(TB6600_TIM_CLK, ENABLE);

    // NOTE:这里不需要配置中断
//    /* 选择中断优先级配置组为4个抢占式优先级和4个子优先级，可以参考misc.h文件了解相关设置 */
//    NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);

//    /* 使能KEY1所在的外部中断通道 */
//    NVIC_InitStructure.NVIC_IRQChannel = TIM1_CC_IRQn;
//    /* 设置抢占式优先级为2 */
//    NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0x00;
//    /* 设置子优先级为3 */
//    NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0x01;
//    /* 使能外部中断通道 */
//    NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
//    /* 初始化配置嵌套向量中断控制器 */
//    NVIC_Init(&NVIC_InitStructure);

    /* 定时器基本参数始终 */
    /* 这里固定设置为：0xFFFF，对本例程无影响 */
    TIM_TimeBaseStructure.TIM_Period = 0xFFFF;
    /* 设置预分频：不预分频，即为72MHz,输出脉冲频率：72MHz/4=18MHz */
    /* 同一个定时器的不同通道PWM的频率只能是相同的，不同通道可以设置为不同的占空比 */
    TIM_TimeBaseStructure.TIM_Prescaler = 4 - 1;
    /* 设置时钟分频系数：不分频(这里用不到) */
    TIM_TimeBaseStructure.TIM_ClockDivision = TIM_CKD_DIV1;
    /* 向上计数模式 */
    TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
    TIM_TimeBaseInit(TB6600_TIMx, &TIM_TimeBaseStructure);

    /* 定时器输出通道1模式配置 */
    /* 模式配置：PWM模式1 */
    TIM_OCInitStructure.TIM_OCMode = TIM_OCMode_Toggle;
    /* 输出状态设置：使能输出 */
    TIM_OCInitStructure.TIM_OutputState = TIM_OutputState_Enable;
    /* 设置跳变值，当计数器计数到这个值时，电平发生跳变 */
    TIM_OCInitStructure.TIM_Pulse = prescaler;
    /* 当定时器计数值小于CCR1_Val时为高电平 */
    TIM_OCInitStructure.TIM_OCPolarity = TIM_OCPolarity_High;
    TIM_OCInitStructure.TIM_OCIdleState = TIM_OCIdleState_Set;
    TIM_OCInitStructure.TIM_OCNIdleState = TIM_OCIdleState_Reset;
    /* 初始化定时器通道1输出PWM */
    TIM_OC2Init(TB6600_TIMx, &TIM_OCInitStructure);

    /* 定时器比较输出通道1预装载配置：使能预装载 */
    TIM_OC2PreloadConfig(TB6600_TIMx, TIM_OCPreload_Disable);

    TIM_DMACmd(TB6600_TIMx, TIM_DMA_CC2, ENABLE);

    /* 使能定时器 */
//    TIM_Cmd(TB6600_TIMx, ENABLE);

/* TIM主输出使能，NOTE:TIM5不支持 */
//    TIM_CtrlPWMOutputs(TB6600_TIMx, ENABLE);
}

/**
  * @brief  Configures the DMA.
  * @param  None
  * @retval None
  */
void DMA_Configuration(void)
{
    DMA_InitTypeDef DMA_InitStructure;

    /* DMA1 Channel5 Config */
    DMA_DeInit(DMA1_Channel5);

    DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)TIM5_CCR2_Address;
    DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)Speed.SpeedTab;
    DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralDST;
    DMA_InitStructure.DMA_BufferSize = 3;
    DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
    DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
    DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;
    DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_HalfWord;
    DMA_InitStructure.DMA_Mode = DMA_Mode_Circular;
    DMA_InitStructure.DMA_Priority = DMA_Priority_High;
    DMA_InitStructure.DMA_M2M = DMA_M2M_Disable;

    DMA_Init(DMA1_Channel5, &DMA_InitStructure);
    DMA_ITConfig(DMA1_Channel5, DMA_IT_TC, ENABLE);

    /* DMA1 Channel5 enable */
    DMA_Cmd(DMA1_Channel5, ENABLE);
}

/**
  * @brief  This function handles DMA1 Channel 6 interrupt request.
  * @param  None
  * @retval None
  */
void DMA1_Channel5_IRQHandler(void)
{
  /* Test on DMA1 Channel6 Transfer Complete interrupt */
  if(DMA_GetITStatus(DMA1_IT_TC5))
  {
    /* Get Current Data Counter value after complete transfer */
//    CurrDataCounterEnd = DMA_GetCurrDataCounter(DMA1_Channel5);
    /* Clear DMA1 Channel6 Half Transfer, Transfer Complete and Global interrupt pending bits */
    DMA_ClearITPendingBit(DMA1_IT_GL6);
  }
}
/**
  * 函数功能: TIMx 输出PWM信号初始化
  * 输入参数: 无
  * 返 回 值: 无
  * 说    明：只要调用这个函数TIMx的四个通道就会有PWM信号输出
  */
void TB6600_TIMx_PWM_Init(void)
{
    TB6600_GPIO_Config();
    TB6600_TIMx_Configuration();
    //    DMA_Configuration();  // NOTE:这里先不要初始化，因为DMA内存地址还没有初始化，且不要启动DMA
}

/* 定时比较中断服务函数 */
void TIM1_CC_IRQHandler(void)
{
    if (TIM_GetITStatus(TB6600_TIMx, TIM_IT_CC1) != RESET)
    {
        TIM_ClearITPendingBit(TB6600_TIMx, TIM_IT_CC1);
        TIM_SetCounter(TB6600_TIMx, 0);
        TIM_SetCompare1(TB6600_TIMx, prescaler);
        pulse_count++;
    }
}

/**
  * 函数功能： 速度表计算函数
  * 输入参数： V0      初速度，单位：转/min
  *           Vt      末速度，单位：转/min
  *           Time    加速时间 单位: s
  * 返 回 值：无
  * 说    明: 根据速度曲线和加速时间,将数据密集化,即计算每一步的速度值,并存放在内存当中
  *           这里采用的数学模型是匀变速直线运动
  *           加速段的曲线有两部分组成,第一是加速度递增的加加速段,
  *           第二是加速度递减的减加速段,两段曲线可以视为关于中心对称(中心是中点速度).这两段曲线所用时间相等
  *           所以中点速度 Vm = (Vt + Vo)/2;
  *       加加速段:
  *           加加速段的加速度曲线是一条过原点的递增的直线,对加速度积分得到的就是速度的增加量
  *           所以有:Vm - Vo = 1/2 * Jerk * t^2,得到加加速度Jerk.
  *           最后得到位移方程 S = 1/6 * Jerk * t^3
  *       减加速段:
  *           减加速段的曲线跟关于加加速段对称,速度公式V = Vt - 1/2 * Jerk * (T-t)^2,
  *           位移公式则需要通过对速度曲线求积分的方法得到,S = Vt * t - 1/6 * Jerk * t^3
  *
  *           :分析过程请结合工程文件夹下的曲线图理解.
  *
  */
uint8_t CalcSpeed(int32_t Vo, int32_t Vt, float Time)
{
    uint8_t Is_Dec = FALSE;
    int32_t i = 0;
    int32_t Vm = 0;              // 中间点速度
    int32_t INCACCELStep;       // 加加速所需的步数
    int32_t DecAccelStep;       // 减加速所需的步数
    int32_t AccelStep;          // 加速或减速需要的步数
    float Jerk = 0;             // 加加速度
    float Ti = 0;               // 时间间隔 dt
    float Sumt = 0;             // 时间累加量
    float DeltaV = 0;           // 速度的增量dv  
    float TiCube = 0;           // 时间间隔的立方
    float* VelocityTab = NULL;  // 速度表格指针

    if (Vo > Vt)                          // 初速度比末速度大,做减速运动,数值变化跟加速运动相同,
    {                                     // 只是建表的时候注意将速度倒序.    
        Is_Dec = TRUE;
        Speed.Vo = ROUNDPS_2_STEPPS(Vt);    // 起速:Step/s
        Speed.Vt = ROUNDPS_2_STEPPS(Vo);    // 末速:Step/s
    }
    else
    {
        Is_Dec = FALSE;
        Speed.Vo = ROUNDPS_2_STEPPS(Vo);
        Speed.Vt = ROUNDPS_2_STEPPS(Vt);
    }

    Time = ACCEL_TIME(Time);                                    // 得到加加速段的时间
    Vm = MIDDLEVELOCITY(Speed.Vo, Speed.Vt);                // 计算中点速度
    Jerk = fabs(INCACCEL(Speed.Vo, Vm, Time));                // 根据中点速度计算加加速度
    INCACCELStep = (int32_t)INCACCELSTEP(Jerk, Time);            // 加加速需要的步数
    DecAccelStep = (int32_t)(Speed.Vt * Time - INCACCELStep);   // 减加速需要的步数 S = Vt * Time - S1

    /* 申请内存空间存放速度表 */
    AccelStep = DecAccelStep + INCACCELStep;              // 加速需要的步数 
    if (AccelStep % 2 != 0)     // 由于浮点型数据转换成整形数据带来了误差,所以这里加1
        AccelStep += 1;
    /* mallo申请内存空间,记得释放 */
    VelocityTab = (float*)(malloc((AccelStep + 1) * sizeof(float)));
    if (VelocityTab == NULL)
    {
        DBG_FATAL("Memory leak!Please modify the curve parameters or the Heap size");
        return FALSE;
    }
    /*
     * 目标的S型速度曲线是对时间的方程,但是在控制电机的时候则是以步进的方式控制,所以这里对V-t曲线做转换,
     * 得到V-S曲线,计算得到的速度表是关于步数的速度值.使得步进电机每一步都在控制当中.
     */
     // 计算第一步速度  //根据第一步的速度值达到下一步的时间
    TiCube = 6.0f * 1.0f / Jerk;                 // 根据位移和时间的公式S = 1/2 * J * Ti^3 第1步的时间:Ti^3 = 6 * 1 / Jerk ;
    Ti = pow(TiCube, (1 / 3.0f));                 // Ti
    Sumt += Ti;
    DeltaV = 0.5f * Jerk * pow(Sumt, 2);           //第一步的速度
    VelocityTab[0] = Speed.Vo + DeltaV;

    if (VelocityTab[0] <= SPEED_MIN)             //以当前定时器频率所能达到的最低速度
        VelocityTab[0] = SPEED_MIN;

    for (i = 1; i < AccelStep; i++)
    {
        /* 步进电机的速度就是定时器脉冲输出频率,可以计算出每一步的时间 */
        /* 得到第i-1步的时间 */
        Ti = 1.0f / VelocityTab[i - 1];             // 电机每走一步的时间 Ti = 1 / Vn-1
       /* 加加速段速度计算 */
        if (i < INCACCELStep)
        {
            Sumt += Ti;//从0开始到i的时间累积
            DeltaV = 0.5f * Jerk * pow(Sumt, 2);            // 速度的变化量: dV = 1/2 * Jerk * Ti^2;
            VelocityTab[i] = Speed.Vo + DeltaV;            // 得到加加速段每一步对应的速度
            // 当最后一步的时候,时间并不严格等于Time,所以这里要稍作处理,作为减加速段的时间
            if (i == INCACCELStep - 1)
                Sumt = fabs(Sumt - Time);
        }
        /* 减加速段速度计算 */
        else
        {
            Sumt += Ti;                                        // 时间累计
            DeltaV = 0.5f * Jerk * pow(fabs(Time - Sumt), 2);  // dV = 1/2 * Jerk *(T-t)^2;
            VelocityTab[i] = Speed.Vt - DeltaV;                // V = Vt - DeltaV ;
            if (VelocityTab[i] >= Speed.Vt)
            {
                AccelStep = i;
                break;
            }
        }
    }
    if (Is_Dec == TRUE) // 减速
    {
        float tmp_Speed = 0;
        Speed.DecelTab = VelocityTab;
        Speed.DecelStep = AccelStep;
        /* 倒序排序 */
        for (i = 0; i < (AccelStep / 2); i++)
        {
            tmp_Speed = VelocityTab[i];
            VelocityTab[i] = VelocityTab[AccelStep - 1 - i];
            VelocityTab[AccelStep - 1 - i] = tmp_Speed;
        }
    }
    else              // 加速
    {
        Speed.AccelTab = VelocityTab;
        Speed.AccelStep = AccelStep;
    }
    //  for(i = 0 ;i <AccelStep;i++)
    //  {
    //    printf("Speed[%d] = %f\n",i,VelocityTab[i]);
    //  }
    return TRUE;
}

/*
*************************************************************************************
  * 函数功能: 步进电机做S型加减速运动
  * 输入参数: Vo:初速度  Vt:末速度 AcTime:加速时间  DeTime:减速时间 Step:步数
  * 返 回 值: 无
  * 说    明: 实现S型加减速定长移动.
*************************************************************************************
*/
void STEPMOTOR_MoveRel(int32_t Vo, int32_t Vt, float AcTime, float DeTime, int32_t Step)
{
    //  if(MotionStatus != WAIT)
    //    return;
    if (CalcSpeed(Vo, Vt, AcTime) == FALSE)// 计算出加速段的速度
        return;
    if (CalcSpeed(Vt, Vo, DeTime) == FALSE)// 计算出减速段的速度
        return;

    if (Step >= (Speed.DecelStep + Speed.AccelStep))
    {
        Speed.Step = Step;
        Speed.DecPoint = Speed.Step - Speed.DecelStep;
    }
    else        //步数不足以进行足够的加减速
    {
        DBG_ERROR("参数设置错误!\n");
        return;
    }

    Speed.Step_Position = 0;
    MotionStatus = ACCEL; // 电机为运动状态

    Speed.ptr = Speed.AccelTab;
    Toggle_Pulse = (uint32_t)(T1_FREQ / (*Speed.ptr));
    Speed.ptr++;
    TIM_SetCounter(TB6600_TIMx, 0);
    TIM_SET_COMPARE(TB6600_TIMx, TB6600_TIM_CHANNEL_x, (uint16_t)(Toggle_Pulse)); // 设置定时器比较值
    TIM_CCxCmd(TB6600_TIMx, TB6600_TIM_CHANNEL_x, TIM_CCx_Enable);// 使能定时器通道 
    STEPMOTOR_OUTPUT_ENABLE();
}

#define V_END                    60           // 末速度 (r/min)
#define V_START                  0            // 初速度 (r/min)
#define ACCELTIME                2.0f         // 加速时间 (s)
#define DECEELTIME               2.0f         // 减速时间 (s)
#define DISTANCE                 5*MICRO_STEP*FSPR        // 距离: 5圈

void StepMotor_Test(void)
{
    /* 无限循环 */
    while (1)
    {
        STEPMOTOR_DIR_FORWARD();
        /* 从 0r/min 加速到60r/min */
        STEPMOTOR_MoveRel(V_START, V_END, ACCELTIME, DECEELTIME, DISTANCE);

        STEPMOTOR_DIR_REVERSAL();
        STEPMOTOR_MoveRel(V_START, V_END, ACCELTIME, DECEELTIME, DISTANCE);
    }
}
